///////////////巡检机器人导航程序///////////////
// All rights reserved by JerryXu--2025.7.30
// 功能：
// 1. 读取参数，获取航点列表
// 2. 连接导航Action服务器，发送导航请求
// 3. 接收导航结果，播放语音，保存图像
// 4. 按顺序循环导航前往所有航点
// 5. 保存目标航点的巡检图像
// 6. 退出程序
// 注意：
// 1. 导航Action服务器需要启动，并在参数服务器中设置相应参数
// 2. 语音播放服务需要启动，并在参数服务器中设置相应参数
// 3. 图像保存路径需要在参数服务器中设置
// 4. 结束signal机制：导航过程中触发会完成当前航点的导航。
// All rights reserved by JerryXu--2025.7.30
////////////////巡检机器人导航程序///////////////
#include <memory>
#include <vector>
#include <string>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <filesystem>  // C++17文件系统
#include <mutex>       
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/msg/image.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "/home/jette/virsual_ws/src/xunjian_robot/include/xunjian_robot/speaker.hpp"  // 自定义服务接口
using NavigateToPose = nav2_msgs::action::NavigateToPose;//单点导航Action
using GoalHandleNav = rclcpp_action::ClientGoalHandle<NavigateToPose>;//单点导航Action的目标句柄

//创建功能实现的类，继承节点类，如果是pyhon直接继承BasicNavigation类即可
class PatrolRobot : public rclcpp::Node {
public:
  // using PlayAudio = xunjian_robot::srv::PlayAudio;

  PatrolRobot() : Node("patrol_robot") {
    init();//服务初始化
    load_waypoints();//参数初始化
    set_initial_point(initial_point);//设置初始位置
    start_patrol();
  }

private:
  // 导航相关
  rclcpp_action::Client<NavigateToPose>::SharedPtr nav_action_client_;
  std::vector<geometry_msgs::msg::PoseStamped> waypoints_;//航点列表
  std::string image_save_path_;
  size_t current_waypoint_ = 0;
  geometry_msgs::msg::PoseStamped initial_point;
  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Client<autopatrol_interfaces::srv::SpeechText>::SharedPtr audio_client_;  // 语音服务客户端
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
  cv::Mat latest_image;
  std::mutex image_mutex; // 用于线程安全
  int cir_count = 0;
  
  //初始化函数
  void init() {
    // 1. 初始化导航参数：Parameter机制
    // (参数服务器：将参数存储在ROS2的全局参数服务器中，可以被所有节点访问)：支持运行时修改（ros2 param set 或 rclcpp::Node::set_parameter）
    declare_parameter("waypoints", std::vector<std::string>{});//将waypoints参数注册，并默认值为空数组
    declare_parameter("initial_waypoint", rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));//初始位置标定
    // 2. 初始化图像保存路径参数:run启动，当前目录就是工作目录；launch启动时，工作目录通常是 launch 文件所在的目录
    declare_parameter("image_save_path", "./src/xunjian_robot/patrol_images/");
    // 2. 初始化导航Action客户端：Action机制
    nav_action_client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
    // 3. 初始化语音服务客户端: Service机制
    audio_client_ = create_client<autopatrol_interfaces::srv::SpeechText>("/speech_text");
    // 4. 初始化图像订阅: topic机制
    image_sub_ = create_subscription<sensor_msgs::msg::Image>(
    "/camera/image_raw", 10,
    [&](const sensor_msgs::msg::Image::SharedPtr msg) {
        image_callback(msg);
    });

  }
  
  // 加载初始点和航点参数
  void load_waypoints() {
    
    auto initial_waypoint = get_parameter("initial_waypoint").as_double_array();//向量参数
    initial_point.header.frame_id = "map";
    initial_point.pose.position.x = initial_waypoint[0];
    initial_point.pose.position.y = initial_waypoint[1];
    double theta = initial_waypoint[2];
    initial_point.pose.orientation.z = sin(theta/2);
    initial_point.pose.orientation.w = cos(theta/2);
  
    auto waypoint_names = get_parameter("waypoints").as_string_array();
    for (const auto& name : waypoint_names) {
      // RCLCPP_INFO(get_logger(), "开始导入");
      declare_parameter(name + ".x", 0.0);
      declare_parameter(name + ".y", 0.0);
      declare_parameter(name + ".yaw", 0.0);  // 朝向角度

      geometry_msgs::msg::PoseStamped pose;
      pose.header.frame_id = "map";
      pose.pose.position.x = get_parameter(name + ".x").as_double();
      pose.pose.position.y = get_parameter(name + ".y").as_double();
      
      // 角度转四元数
      double theta = get_parameter(name + ".yaw").as_double();
      pose.pose.orientation.z = sin(theta / 2);
      pose.pose.orientation.w = cos(theta / 2);

      waypoints_.push_back(pose);
      RCLCPP_INFO(get_logger(), "载入航点： %s: (%.2f, %.2f, %.2f)",
                 name.c_str(), pose.pose.position.x, 
                 pose.pose.position.y, theta);
    }
  }
  
  // 设置初始位置
  void set_initial_point(geometry_msgs::msg::PoseStamped initial_point) {
    initial_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
    geometry_msgs::msg::PoseWithCovarianceStamped msg;
    msg.header = initial_point.header;
    msg.pose.pose = initial_point.pose;
    initial_pose_pub_->publish(msg);
    // 使用正确的变量名initial_point
    RCLCPP_INFO(this->get_logger(), "初始位姿设置完成: x=%.2f, y=%.2f, theta=%.2f", 
               initial_point.pose.position.x, 
               initial_point.pose.position.y,
               atan2(2*(initial_point.pose.orientation.w*initial_point.pose.orientation.z), 
                      1-2*(initial_point.pose.orientation.z*initial_point.pose.orientation.z)));
    
    RCLCPP_INFO(this->get_logger(), "等待导航系统初始化...");
    std::this_thread::sleep_for(std::chrono::seconds(2));
    play_announcement("服务初始化完成");
  }

  // 开始巡航
  void start_patrol() {
    if (waypoints_.empty()) {
      RCLCPP_ERROR(get_logger(), "未导入任何航路点!");
      return;
    }
    navigate_to_waypoint();
    play_announcement("开始导航");
  }

  // 循环巡检
  void navigate_to_waypoint() {
    
    while (!nav_action_client_->wait_for_action_server(std::chrono::seconds(5))) {
      RCLCPP_WARN(get_logger(), "导航系统初始化未完成，等待中...");
      if (!rclcpp::ok()) {
      return;  // 如果ROS已关闭，直接退出
      continue;
    }
    }
    
    auto goal_msg = NavigateToPose::Goal();
    goal_msg.pose = waypoints_[current_waypoint_]; 
    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    auto last_feedback_time_ = this->now();

    // Corrected goal response callback
    send_goal_options.goal_response_callback =
    [this](std::shared_future<GoalHandleNav::SharedPtr> future) {
        auto goal_handle = future.get();
        if (goal_handle) {
            RCLCPP_INFO(get_logger(), "目标点 %zu 已被服务器接收", current_waypoint_);
            RCLCPP_INFO(get_logger(), "正在前往目标点： %zu ... ", current_waypoint_);
        } else {
            RCLCPP_INFO(get_logger(), "目标点 %zu 已被服务器拒绝", current_waypoint_);
        }
    };
    
    //注释打开会时刻返回导航状态信息
    // send_goal_options.feedback_callback = 
    // [this](GoalHandleNav::SharedPtr,const std::shared_ptr<const NavigateToPose::Feedback>feedback) {
    //     RCLCPP_INFO(get_logger(), "目标点 %zu 剩余距离: %.2f 米", current_waypoint_, feedback->distance_remaining);
    // };

    send_goal_options.result_callback =
      [this](const GoalHandleNav::WrappedResult& result) {
        switch (result.code) {
          case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(get_logger(), "到达航路点： %zu", current_waypoint_);  
          // 1. 播放语音
          if (cir_count != 0) {
            play_announcement("Arrived at waypoint " + std::to_string(current_waypoint_));}
          // 2. 保存当前图像
            save_current_image();
          // 3. 循环移动到下一个航点（巧妙的数学运算）
            current_waypoint_ = (current_waypoint_ + 1) % waypoints_.size();
          // 4.循环巡航或终止
            if (current_waypoint_ == 0) {
              cir_count++;
              RCLCPP_INFO(get_logger(), "本轮巡检完毕，正在返回起点...");
              play_announcement("本轮巡检完毕，正在返回起点..."); 
            }
            if(current_waypoint_ == 1 && cir_count!=0){
              RCLCPP_INFO(get_logger(), "返回起点成功，完成第 %d 轮巡检...",cir_count);
              play_announcement("返回起点成功，完成第 " + std::to_string(cir_count) + " 轮巡检... ");
            }
            navigate_to_waypoint();
            break;
          case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(get_logger(), "导航被中止");
            break;
          case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(get_logger(), "导航被取消");
            play_announcement("导航被取消... ");
            break;
          default:
            RCLCPP_ERROR(get_logger(), "导航失败"); 
            break;
        }};
    nav_action_client_->async_send_goal(goal_msg, send_goal_options);
    RCLCPP_INFO(get_logger(), " 准备导航前往航路点 %zu ", current_waypoint_);


  };
  
  // 语音播放服务
  void play_announcement(const std::string & text) {
    
    while (!audio_client_->wait_for_service(std::chrono::seconds(1))) {
    if (!rclcpp::ok()) {
        RCLCPP_INFO(get_logger(), "等待语音过程中ROS已关闭, 退出程序...");
        return;
    }
    RCLCPP_INFO(this->get_logger(), "语音合成服务未上线，等待中...");
    }

    auto request = std::make_shared<autopatrol_interfaces::srv::SpeechText::Request>();
    request->text = text;
    auto response_received_callback = [this, text](rclcpp::Client<autopatrol_interfaces::srv::SpeechText>::SharedFuture future) {
    auto result = future.get();
    // RCLCPP_INFO(get_logger(), "语音合成服务调用成功 %s", text.c_str());
    };
  
    auto future = audio_client_->async_send_request(request, response_received_callback);
  
    // 不要使用 spin_until_future_complete!
    // 简单等待一小段时间让请求发送出去
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  // 图像保存回调
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
    // RCLCPP_INFO_ONCE(get_logger(), "图像回调函数被调用!");  // 只打印一次调试使用
    RCLCPP_DEBUG(get_logger(), "接收到图像消息，时间戳: %d.%09d", 
                 msg->header.stamp.sec, msg->header.stamp.nanosec);  
    try {
        // 将ROS图像消息转换为OpenCV Mat
        latest_image = cv_bridge::toCvCopy(msg, "bgr8")->image;
        RCLCPP_DEBUG(get_logger(), "成功接收并转换图像: %dx%d", latest_image.cols, latest_image.rows);  
      } catch (const cv_bridge::Exception& e) {
        RCLCPP_ERROR(get_logger(), "CV桥接转换失败: %s", e.what());
        return;
      } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "处理图像时发生错误: %s", e.what());
        return;}
  }

  // 保存当前图像
  void save_current_image() {
    cv::Mat current_image_copy;
    {
        std::lock_guard<std::mutex> lock(image_mutex);
        if (latest_image.empty()) {
            RCLCPP_WARN(get_logger(), "没有可用的图像数据");
            return;
        }
        current_image_copy = latest_image.clone();
    }
    image_save_path_ = get_parameter("image_save_path").as_string();
    if (image_save_path_.empty()) {
        RCLCPP_WARN(get_logger(), "图像保存路径未设置");
        return;
    }
    if (current_image_copy.empty()) {
        RCLCPP_WARN(get_logger(), "图像数据为空");
        return;
    }
    std::string filename = image_save_path_ + "waypoint_" + 
                          std::to_string(current_waypoint_) + ".jpg";
    // 保存图像
    if (cv::imwrite(filename, current_image_copy)) {
        RCLCPP_INFO(get_logger(), "成功保存图像: %s", filename.c_str());
    } else {
        RCLCPP_ERROR(get_logger(), "保存图像失败: %s", filename.c_str());
    }
  }


};

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<PatrolRobot>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  // try {
  //   auto node = std::make_shared<PatrolRobot>();
  //   rclcpp::spin(node);
  // } catch (const std::exception& e) {
  //   RCLCPP_ERROR(rclcpp::get_logger("main"), "节点异常: %s", e.what());
  // }
  // rclcpp::shutdown();
  return 0;
}